#include "ethernet-parser.h"

namespace eth_parser
{
    class SomeIPSplitterVars : public Vars
    {
    public:
        Array<UInt> someIPStartIndicesBuffer;
        Array<BinarySpan> someIPFrames;
        SomeIPSplitterVars() : someIPStartIndicesBuffer(MAX_SOME_IP_FRAME_COUNT), someIPFrames(MAX_SOME_IP_FRAME_COUNT)
        {}
    };

    UInt getSomeIPID(BinarySpan& someIPFrame)
    {
        if (someIPFrame.size() < 4) return 0;
        Byte *rawData = someIPFrame.data();
        return ((UInt)rawData[0] << 24) | ((UInt)rawData[1] << 16) | ((UInt)rawData[2] << 8) | (UInt)rawData[3];
    }

    Binary genSplittedPacket(BinarySpan header, UInt ipv4StartIndex, BinarySpan someIPFrame)
    {
        UInt packetSize = header.size() + someIPFrame.size();
        Int ipLayerLength = (Int)packetSize - ipv4StartIndex;
        Int udpLayerLength = (Int)packetSize - ipv4StartIndex - 20;
        if (udpLayerLength < 24) return Binary();

        Binary packet(packetSize);
        utility::memoryCopy(header.data(), packet.data(), header.size());
        utility::memoryCopy(someIPFrame.data(), &packet[header.size()], someIPFrame.size());

        packet[ipv4StartIndex + 2] = (Byte)((ipLayerLength & 0xff00) >> 8);
        packet[ipv4StartIndex + 3] = (Byte)(ipLayerLength & 0x00ff);

        packet[ipv4StartIndex + 24] = (Byte)((udpLayerLength & 0xff00) >> 8);
        packet[ipv4StartIndex + 25] = (Byte)(udpLayerLength & 0x00ff);

        return packet;
    }
}

using namespace eth_parser;

Bool EthernetParser::parse(Binary& rawPacket, UInt channel, Array<Enum<BusChannelType> >& typeHints, Enum<BusChannelType>& type, UInt& id)
{
    if (channel <= typeHints.size())
    {
        type = typeHints[channel - 1];
        return parseDirectly(rawPacket, type, id);
    }

    UInt rawSize = rawPacket.size();
    if (rawSize <= 14 || rawSize > 65535) return FALSE; // invalid for normal ethernet packet

    type = BusChannelType::Value::Ethernet;
    id = *(UInt*)&rawPacket[8]; // idByMac
    return TRUE;
}

Bool EthernetParser::parseDirectly(Binary& rawPacket, Enum<BusChannelType> type, UInt& id)
{
    UInt rawSize = rawPacket.size();
    if (rawSize <= 14 || rawSize > 65535) return FALSE; // invalid for normal ethernet packet

    Byte *rawData = rawPacket.data();

    if (type == BusChannelType::Value::Ethernet)
    {
        id = *(UInt*)&rawData[8]; // idByMac
        return TRUE;
    }
    else if (type == BusChannelType::Value::SomeIP)
    {
        UInt someIPStartIndex = 0;
        if (parseSomeIP(rawData, rawSize, someIPStartIndex)) id = parseBigEndianUInt(rawData, someIPStartIndex);
        else id = 0;
        return TRUE;
    }
    else return FALSE;
}

void EthernetParser::splitSomeIPFrames(Binary& rawPacket, Array<UInt>& someIPStartIndicesBuffer, BinarySpan& header, UInt& ipv4StartIndex, Array<BinarySpan>& someIPFrames, UInt& validCount)
{
    if (someIPFrames.isEmpty() || someIPStartIndicesBuffer.isEmpty())
    {
        validCount = 0;
        return;
    }

    UInt rawSize = rawPacket.size();
    if (rawSize <= 14 || rawSize > 65535) // invalid for normal ethernet packet
    {
        validCount = 0;
        return;
    }

    Byte *rawData = rawPacket.data();
    if (!parseSomeIP(rawData, rawSize, someIPStartIndicesBuffer, validCount, ipv4StartIndex) || validCount == 0)
    {
        validCount = 0;
        return;
    }

    header = rawPacket.span(0, someIPStartIndicesBuffer[0]);
    for (UInt i = 0; i < validCount - 1; i++)
    {
        someIPFrames[i] = rawPacket.span(someIPStartIndicesBuffer[i], someIPStartIndicesBuffer[i + 1] - someIPStartIndicesBuffer[i]);
    }
    someIPFrames[validCount - 1] = rawPacket.span(someIPStartIndicesBuffer[validCount - 1]);
}

Word EthernetParser::parseBigEndianWord(Byte *rawData, UInt offset)
{
    return ((UInt)rawData[offset] << 8) | (UInt)rawData[offset + 1];
}

UInt EthernetParser::parseBigEndianUInt(Byte *rawData, UInt offset)
{
    return ((UInt)rawData[offset] << 24) | ((UInt)rawData[offset + 1] << 16) | ((UInt)rawData[offset + 2] << 8) | (UInt)rawData[offset + 3];
}

Bool EthernetParser::parseSomeIP(Byte *rawData, UInt rawSize, UInt& someIPStartIndex)
{
    Array<UInt> someIPStartIndicesBuffer(MAX_SOME_IP_FRAME_COUNT);
    UInt validIndexCount = 0;
    UInt dummy = 0;
    if (parseSomeIP(rawData, rawSize, someIPStartIndicesBuffer, validIndexCount, dummy))
    {
        someIPStartIndex = someIPStartIndicesBuffer[0];
        return TRUE;
    }
    else return FALSE;
}

Bool EthernetParser::parseSomeIP(Byte *rawData, UInt rawSize, Array<UInt> someIPStartIndicesBuffer, UInt& validIndexCount, UInt& ipv4StartIndex)
{
    ipv4StartIndex = 0;
    if (rawData[12] == 8 && rawData[13] == 0) // IPv4
    {
        ipv4StartIndex = 14;
    }
    else if (rawData[12] == 0x81 && rawData[13] == 0) // VLAN tag
    {
        if (rawSize <= 18) return FALSE;
        if (rawData[16] == 8 && rawData[17] == 0) // IPv4
        {
            ipv4StartIndex = 18;
        }
    }

    if (ipv4StartIndex != 0)
    {
        if (rawSize <= ipv4StartIndex + 20) return FALSE;
        
        UInt ipLayerLength = parseBigEndianWord(rawData, ipv4StartIndex + 2);
        if (rawSize < ipv4StartIndex + ipLayerLength) return FALSE;

        if (rawData[ipv4StartIndex + 9] == 17) // UDP
        {
            UInt udpStartIndex = ipv4StartIndex + 20;

            if (rawSize <= udpStartIndex + 8) return FALSE;

            UInt udpLayerLength = parseBigEndianWord(rawData, udpStartIndex + 4);
            if (rawSize < udpStartIndex + udpLayerLength) return FALSE;

            UInt count = 0;
            UInt nextStartIndex = udpStartIndex + 8;
            while (TRUE)
            {
                if (nextStartIndex == rawSize)
                {
                    if (count > 0)
                    {
                        validIndexCount = count;
                        return TRUE;
                    }
                    else return FALSE;
                }

                if (rawSize <= nextStartIndex + 16) return FALSE;

                UInt someIPLayerLength = parseBigEndianUInt(rawData, nextStartIndex + 4);
                if (rawSize < nextStartIndex + 8 + someIPLayerLength) return FALSE;

                if (rawData[nextStartIndex] == 0xff && rawData[nextStartIndex + 1] == 0xff) return FALSE;
                if (rawData[nextStartIndex + 12] != 1 || rawData[nextStartIndex + 14] != 2 || rawData[nextStartIndex + 15] != 0) return FALSE;

                if (count >= someIPStartIndicesBuffer.size()) return FALSE;
                
                someIPStartIndicesBuffer[count++] = nextStartIndex;
                nextStartIndex += 8 + someIPLayerLength;
            }
        }
        else return FALSE;
    }
    else return FALSE;
}

SomeIPSplitter::SomeIPSplitter() : Object<SomeIPSplitterVars>(new SomeIPSplitterVars(), TRUE)
{}

Array<SessionBusRawData> SomeIPSplitter::split(SessionBusRawData someIPData)
{
    UInt ipv4StartIndex = 0;
    UInt validCount = 0;
    BinarySpan header;
    EthernetParser::splitSomeIPFrames(someIPData.binary, vars->someIPStartIndicesBuffer, header, ipv4StartIndex, vars->someIPFrames, validCount);
    Array<SessionBusRawData> result(validCount);
    for (UInt i = 0; i < validCount; i++)
    {
        Binary newPacket = genSplittedPacket(header, ipv4StartIndex, vars->someIPFrames[i]);
        if (newPacket.isEmpty()) continue;

        SessionBusRawData& newData = result[i];
        newData.timestamp = someIPData.timestamp;
        newData.channel = someIPData.channel;
        newData.txFlag = someIPData.txFlag;
        newData.id = getSomeIPID(vars->someIPFrames[i]);
        newData.binary = newPacket;
    }
    return result;
}